/*
 -------------------------------------------------------------------------------
 flightDataLogger.cpp
 
 This program is used to record flight data.
 
 Initially created by Rowland O'Flaherty on 1/10/12.
 
 Version 1.0
 -------------------------------------------------------------------------------
 */

//------------------------------------------------------------------------------
// Includes and Defines
//------------------------------------------------------------------------------
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <BMP085.h>
#include <ITG3200.h>
#include <ADXL345.h>
#include <HMC5843.h>
#include <TinyGPS.h>        // docu: http://arduiniana.org/libraries/tinygps/
#include <Servo.h>
#include <ServoGyro.h>


//------------------------------------------------------------------------------
// Helper Function Declarations
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Global Varible Declarations
//------------------------------------------------------------------------------

// Logging Constants and Variables
int ts = 250; // [ms] Sampling time
int ledPin = 13;

// Logging Constants and Variables
//int LOGRX = 4;
//int LOGTX = 5;
//int LOGBAUD = 9600;
//SoftwareSerial loggerSerial(LOGRX, LOGTX);

// ++++++++++++++++++++++++++++++++++
// GPS Sensor Constants and Variables
// ++++++++++++++++++++++++++++++++++
int GPSRX = 4;
int GPSTX = 5;
int GPSBAUD = 4800;
TinyGPS gps;
SoftwareSerial gpsSerial(GPSRX, GPSTX);

// +++++++++++++++++++++++++++++++++++++++
// Pressure Sensor Constants and Variables
// +++++++++++++++++++++++++++++++++++++++
BMP085 bmp;

// ++++++++++++++++++++++++++++++++++++++++++++
// Accelerometer Sensor Constants and Variables
// ++++++++++++++++++++++++++++++++++++++++++++
ADXL345 adxl;

// +++++++++++++++++++++++++++++++++++
// Gyro Sensor Constants and Variables
// +++++++++++++++++++++++++++++++++++
ITG3200 itg;

// +++++++++++++++++++++++++++++++++++++++++++
// Magnetometer Sensor Constants and Variables
// +++++++++++++++++++++++++++++++++++++++++++
HMC5843 hmc;

// +++++++++++++++++++++++++++++
// Servo Constants and Variables
// +++++++++++++++++++++++++++++
ServoGyro servoRudder;
ServoGyro servoElevator;

// Servo pins
int servoRudderDigitalOut = 6;
int servoRudderAnalogIn = 0;
int servoElevatorDigitalOut = 7;
int servoElevatorAnalogIn = 1;

bool use_gps = true;
bool use_adxl = false;
bool use_bmp = false;
bool use_itg = false;
bool use_hmc = false;
bool use_servos = false;
bool calibrationData = false;

//------------------------------------------------------------------------------
// Setup Function - Initializes Arduino
//------------------------------------------------------------------------------
void setup() {
    pinMode(ledPin, OUTPUT);

    // Setup GPS
    if(use_gps) {
        gpsSerial.begin(GPSBAUD);
    }

    // Setup servos
    if(use_servos) {
        servoRudder.attach(servoRudderDigitalOut);
        servoElevator.attach(servoElevatorDigitalOut);
    
        // calibrate with input data or with the calibration procedure
        if(calibrationData) {
            servoRudder.calibrateWithData(servoRudderAnalogIn,servoRudderDigitalOut, 250, 50);
            servoRudder.calibrateWithData(servoElevatorAnalogIn,servoElevatorDigitalOut, 250, 50);
        } 
        else {
            // JP's Hitec HS-81, reference voltage = 5V??
            // int minVoltage = 80;
            // int maxVoltage = 354;
            // int minPulseWidth = 627;
            // int maxPulseWidth = 2392;
            servoRudder.calibrateWithData(80, 354, 627, 2392);
            servoElevator.calibrateWithData(80, 354, 627, 2392);
        }
    }

    // Initialize accelerometer
    if(use_adxl) {
        adxl.initialize();
    
        //set activity/ inactivity thresholds (0-255)
        adxl.setActivityThreshold(75); //62.5mg per increment
        adxl.setInactivityThreshold(75); //62.5mg per increment
        adxl.setTimeInactivity(10); // how many seconds of no activity is inactive?
    
        //look of activity movement on this axes - 1 == on; 0 == off 
        adxl.setActivityX(1);
        adxl.setActivityY(1);
        adxl.setActivityZ(1);
    
        //look of inactivity movement on this axes - 1 == on; 0 == off
        adxl.setInactivityX(1);
        adxl.setInactivityY(1);
        adxl.setInactivityZ(1);
    }

    // initialize pressure sensor
    if(use_bmp) {
        bmp.initialize();
        bmp.calibrateWithAlt(300);
        bmp.setOSS(3);
    }

    // Initialize gyro
    if(use_itg) {
        itg.initialize();
    }

    // Initialize magnetometer
    if(use_hmc) {
        hmc.initialize();
        hmc.setSamplingRate(4); // 10Hz
        hmc.setGain(0); // 0.7Ga
    }

    // Print header for data logging
    Serial.begin(9600); //This pipes to the serial monitor
    Serial.println("Altitude, Temperature, Pressure, AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ, servoRudderVoltage, servoElevatorVoltage, lat, lon, altitude, course, speed_ms, speed_kmph");
}

//------------------------------------------------------------------------------
// Loop Function - Arduino Program Loop
//------------------------------------------------------------------------------
void loop() {
    digitalWrite(ledPin, HIGH);
    delay(ts-50);
    digitalWrite(ledPin, LOW);
    delay(50);
    
    // Declare local variables
    double accXYZ[3];
    int gyroX, gyroY, gyroZ;
    int magX, magY, magZ;
    float flat, flon, falt, fc, fk, fmph, fmps, fkmph;
    unsigned long fix_age, chars;
    unsigned short sentences, failed_checksum;

    if(use_gps) {
        Serial.print("gpsSerial.available: "); Serial.println(gpsSerial.available());
        while (gpsSerial.available()) {
            int c = gpsSerial.read();
            
            if (gps.encode(c)) {
                Serial.print(c);
            }
            
            // check if the character completes a valid GPS sentence
            gps.stats(&chars, &sentences, &failed_checksum);
            if(sentences == 1){
                break;
            }
        }
  
        // returns +/- latitude/longitude in degrees
        gps.f_get_position(&flat, &flon, &fix_age);
        falt = gps.f_altitude(); // +/- altitude in meters
        fc = gps.f_course(); // course in degrees
        //fk = gps.f_speed_knots(); // speed in knots
        //fmph = gps.f_speed_mph(); // speed in miles/hr
        fmps = gps.f_speed_mps(); // speed in m/sec
        fkmph = gps.f_speed_kmph(); // speed in km/hr   
    }
    // Accelerometer data
    if(use_adxl) {
        adxl.get_Gxyz(accXYZ);
    }

    // Gyro data
    if(use_itg) {
        gyroX = itg.gyroX();
        gyroY = itg.gyroY();
        gyroZ = itg.gyroZ();
    }

    // Magnetometer data
    if(use_hmc) {
        hmc.raw(magX, magY, magZ);
    }

    if(use_bmp) {
        Serial.print(bmp.altitude(),2); 
        Serial.print(", ");
        Serial.print(bmp.temperature(),2); 
        Serial.print(", ");
        Serial.print(bmp.pressure()); 
        Serial.print(", ");
    }

    if(use_adxl) {
        Serial.print(accXYZ[0]); 
        Serial.print(", ");
        Serial.print(accXYZ[1]); 
        Serial.print(", ");
        Serial.print(accXYZ[2]); 
        Serial.print(", ");
    }
    
    if(use_itg) {
        Serial.print(gyroX); 
        Serial.print(", ");
        Serial.print(gyroY); 
        Serial.print(", ");
        Serial.print(gyroZ); 
        Serial.print(", ");
    }

    if(use_hmc) {
        Serial.print(magX); 
        Serial.print(", ");
        Serial.print(magY); 
        Serial.print(", ");
        Serial.print(magZ); 
        Serial.print(", ");
    }
    
    if(!use_gps) {
        Serial.print(flat); 
        Serial.print(", ");
        Serial.print(flon); 
        Serial.print(", ");
        Serial.print(falt); 
        Serial.print(", ");
        Serial.print(fc); 
        Serial.print(", ");
        //Serial.print(fmph); 
        //Serial.print(", ");
        //Serial.print(fmps);
        //Serial.print(", ");
        Serial.print(fkmph); 
        Serial.print(", ");
    }

    if(use_servos) {
        Serial.print(analogRead(servoRudderAnalogIn)); 
        Serial.print(", ");
        Serial.print(analogRead(servoElevatorAnalogIn)); 
        Serial.print(", ");
    }

    Serial.println();
}

//------------------------------------------------------------------------------
// Helper Functions
//------------------------------------------------------------------------------
    
    //loggerSerial.begin(LOGBAUD);
    //loggerSerial.println("Altitude, Temperature, Pressure, AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ");

    // Print calibration data for gyro for data logging
    //loggerSerial.println("gyroCalX, gyroCalY, gyroCalZ");
    //loggerSerial.println(itg.getCalX()); loggerSerial.print(", ");
    //loggerSerial.println(itg.getCalY()); loggerSerial.print(", ");
    //loggerSerial.println(itg.getCalZ()); loggerSerial.print(", ");

    //    loggerSerial.print(bmp.altitude(),2); loggerSerial.print(", ");
    //    loggerSerial.print(bmp.temperature(),2); loggerSerial.print(", ");
    //    loggerSerial.print(bmp.pressure()); loggerSerial.print(", ");
    //    
    //    loggerSerial.print(accXYZ[0]); loggerSerial.print(", ");
    //    loggerSerial.print(accXYZ[1]); loggerSerial.print(", ");
    //    loggerSerial.print(accXYZ[2]); loggerSerial.print(", ");
    //    
    //    loggerSerial.print(gyroX); loggerSerial.print(", ");
    //    loggerSerial.print(gyroY); loggerSerial.print(", ");
    //    loggerSerial.print(gyroZ); loggerSerial.print(", ");
    //    
    //    loggerSerial.print(magX); loggerSerial.print(", ");
    //    loggerSerial.print(magY); loggerSerial.print(", ");
    //    loggerSerial.print(magZ); loggerSerial.print(", ");
    //    loggerSerial.println();
    //    
    //    loggerSerial.println();
